perm filename DISTOR.SAI[VIS,HPM]3 blob
sn#249040 filedate 1976-11-19 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00005 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00002 00002 ENTRY
C00007 00003 PQINIT computes several quantities depending on the order of the
C00013 00004 DISCAL uses the reference positions X and Y and the distorted
C00022 00005 DISREM uses the distortion calibration coefficients G as computed by
C00025 ENDMK
C⊗;
ENTRY;
BEGIN "DISTOR"
REQUIRE "DUBBLE.SAI[NUM,HPM]" SOURCE_FILE;
REQUIRE "DDHDR.SAI[GRA,HPM]" SOURCE_FILE;
DEFINE MMMM="2";
REQUIRE "ACCEL.SAI[1,DBG]" SOURCE_FILE;
DEFINE CRLF="'15&'12";
REAL XLO,YLO,XHI,YHI,SWW,SH;
INTEGER PP,PH,MG,MR,M1,M2,M3,M4,M;
INTEGER I,J,K;
COMMENT VERT returns a string which consists of the string SI with a CRLF inserted
after each character;
SIMPLE STRING PROCEDURE VERT (STRING SI);
BEGIN
STRING SO;
SO←NULL;
WHILE SI≠NULL DO SO ← SO & LOP(SI) & CRLF;
RETURN(SO)
END;
COMMENT DMODEL uses the values of X and Y to compute the coefficients of
the parameters and inserts them into the A and B arrays, which
can be used to obtain the corrections in X and Y, respectively;
SIMPLE PROCEDURE DMODEL (REAL ARRAY A,B; REAL X,Y);
BEGIN
REAL XX,XY,YY,YDX,RR,RRX,RRY;
IF X=0 THEN YDX←0 ELSE YDX←Y/X;
I←1;
A[1]←B[M1]←XX←1;
YY←1;
FOR J←1 STEP 1 UNTIL PP DO
BEGIN
I←I+1;
XX←XX*X;
YY←YY*Y;
A[I]←B[MG+I]←XY←XX;
FOR K←1 STEP 1 UNTIL J-1 DO
BEGIN
I←I+1;
A[I]←B[MG+I]←XY←XY*YDX;
END;
I←I+1;
A[I]←B[MG+I]←YY;
END;
IF MR>0 THEN
BEGIN
RR ← X↑2 + Y↑2;
XY←RR↑PH;
RRX←X*XY;
RRY←Y*XY;
A[M3]←RRX;
B[M3]←RRY;
FOR I←M4 STEP 1 UNTIL M DO
BEGIN
A[I] ← RRX ← RRX*RR;
B[I] ← RRY ← RRY*RR;
END;
END;
END;
SIMPLE PROCEDURE BORDER;
BEGIN
LITEN;
LINE(-XHI,-YHI,-XHI,YHI);
LINE(-XHI,YHI,XHI,YHI);
LINE(XHI,YHI,XHI,-YHI);
LINE(XHI,-YHI,-XHI,-YHI);
LINE(-SWW,-SH,-SWW,SH);
LINE(-SWW,SH,SWW,SH);
LINE(SWW,SH,SWW,-SH);
LINE(SWW,-SH,-SWW,-SH)
END;
COMMENT LELLIPSE displays an open ellipse with center at X and Y, semimajor axis
A, semiminor axis B, and orientation THETA (counterclockwise relative
to the X axis);
SIMPLE PROCEDURE LELLIPSE (REAL X,Y,A,B,THETA);
BEGIN
PRELOAD_WITH 0,.2599,.5021,.7101,.8697,.9701,1.0043,.9701,.8697,.7101,.5021,.2599,
0,-.2599,-.5021,-.7101,-.8697,-.9701,-1.0043,-.9701,-.8697,-.7101,-.5021,-.2599,
0,.2599,.5021,.7101,.8697,.9701,1.0043;
SAFE OWN REAL ARRAY XR[-6:24];
REAL S,C,CA,CB,SA,SB,X1,Y1,X2,Y2,YR;
S←SIN(THETA);
C←COS(THETA);
CA←C*A;
CB←C*B;
SA←S*A;
SB←S*B;
X1←CA*XR[0]+X;
Y1←SA*XR[0]+Y;
FOR I←1 STEP 1 UNTIL 24 DO
BEGIN
YR←XR[I-6];
X2←CA*XR[I]-SB*YR+X;
Y2←SA*XR[I]+CB*YR+Y;
LINE(X1,Y1,X2,Y2);
X1←X2;
Y1←Y2;
END;
END;
COMMENT PQINIT computes several quantities depending on the order of the
polynominals and needed in subsequent calculations;
SIMPLE PROCEDURE PQINIT (INTEGER P,Q);
BEGIN
PP←P;
PH ← (P+1) DIV 2;
MG ← (P+1)*(P+2)%2;
MR ← 0 MAX ((Q+1) DIV 2 - PH);
M1←MG+1;
M2←2*MG;
M3←M2+1;
M4←M3+1;
M←M2+MR
END;
COMMENT SETUP initializes the display and assigns a video channel, returning
its number as CHAN. SW should be such that the points to be plotted
(as in VECTRA or ELLIPA) will be between -SW and SW in X and between
-.75*SW and .75*SW in Y;
SIMPLE PROCEDURE SETUP (REAL SW; REFERENCE INTEGER CHAN);
BEGIN
DDINIT;
SWW←SW;
SH←.75*SW;
XLO←-1.3*SW; YLO←-.95*SW; XHI←1.1*SW; YHI←.85*SW;
SCREEN(XLO,YLO,XHI,YHI);
COMMENT PPPOS(YLO,-.8*SW);
CHAN←GDDCHN(-1);
OUTSTR("CHANNEL " & CVOS(CHAN) & " ASSIGNED" & CRLF);
DPYUP(CHAN); DPYUP(CHAN); DPYUP(CHAN);
SHOWA(CHAN);
END;
COMMENT VECTRA displays on channel CHAN the N vectors XV,YV with
origins at X,Y. The vectors are multiplied by scale factors
typed in. Typing "X" causes the current display to be output
to the XGP. Typing only a carriage return causes exit from
the procedure. NAME is used for identifying the quantity plotted.
IDENT is used as an additional label on the XGP output (along with
the date and time);
PROCEDURE VECTRA (STRING IDENT,NAME; INTEGER CHAN,N; REAL ARRAY X,Y,XV,YV;
REAL SCALE(1.0));
BEGIN
REAL R;
INTEGER PNT;
R←.006*XHI;
FOR PNT←1 STEP 1 UNTIL N DO
BEGIN
ELLIPS(X[PNT]-R,Y[PNT]-R,X[PNT]+R,Y[PNT]+R);
LINE(X[PNT],Y[PNT],X[PNT]+SCALE*XV[PNT],Y[PNT]+SCALE*YV[PNT])
END;
BORDER;
TXTPOS(-1.09*XHI,.64*XHI,.05*XHI,.075*XHI);
textd(VERT(CVS(SCALE)&" * " & NAME));
DPYUP(CHAN); DPYUP(CHAN); DPYUP(CHAN);
END "VECTRA";
COMMENT ELLIPA displays on channel CHAN the N ellipses with centers at X,Y
and defined by the covariance matrices propagated from S according to the
calibration model computed from P and Q as in DISCAL. Scale factors,
labels, and XGP output are used as in VECTRA;
PROCEDURE ELLIPA (STRING IDENT,NAME; INTEGER CHAN,P,Q,N; REAL ARRAY X,Y,S);
BEGIN
REAL ARRAY A,B[1:44],AB[1:2,1:44];
SAFE REAL ARRAY CV[1:2,1:2],SMAJ,SMIN,THETA[1:N];
REAL SUMV,DIFV,RAD,SCALE;
INTEGER PNT;
PQINIT(P,Q);
FOR PNT←1 STEP 1 UNTIL N DO
BEGIN
DMODEL(A,B,X[PNT],Y[PNT]);
FOR I←1 STEP 1 UNTIL M DO
BEGIN
AB[1,I]←A[I];
AB[2,I]←B[I]
END;
ERRPROP(2,M,CV,AB,S);
SUMV ← CV[1,1]+CV[2,2];
DIFV ← CV[1,1]-CV[2,2];
RAD ← SQRT(DIFV↑2 + 4*CV[1,2]↑2);
SMAJ[PNT] ← SQRT((SUMV+RAD)/2);
SMIN[PNT] ← SQRT((SUMV-RAD)/2);
THETA[PNT] ← ATAN2(2*CV[1,2],DIFV)/2
END;
SCALE←10;
FOR PNT←1 STEP 1 UNTIL N DO
LELLIPSE(X[PNT],Y[PNT],SCALE*SMAJ[PNT],SCALE*SMIN[PNT],THETA[PNT]);
BORDER;
TXTPOS(-1.09*XHI,.64*XHI,.05*XHI,.075*XHI);
textd(VERT("10 * " & NAME));
DPYUP(CHAN); DPYUP(CHAN); DPYUP(CHAN);
END "ELLIPA";
COMMENT DISCAL uses the reference positions X and Y and the distorted
positions XP and YP of N image points to compute the distortion
calibration polynomial coefficients G (for converting X,Y to XP,YP)
and their covariance matrix S. (To allow P and Q to have their
maximum values, G should be dimensioned [1:44] and S should be
dimensioned [1:44,1:44].) P is the degree of the two-dimensional
polynomials for general distortion, and Q is the degree of the
one-dimensional polynomial for radial distortion. (An even value
of Q is equivalent to the next lower odd value. All values of Q
such that Q≤P are equivalent.) SW is the image semiwidth as in SETUP.
SD is the computed standard deviation of the observation errors (unmodeled
errors in X and Y). If on input 0≤P≤5 and Q≤10, these values are
accepted. Otherwise, typed-in values P and Q are asked for, with
the entire process repeating until only a carriage return is
typed for P. The final values are returned. The distortion,
residuals, and propagated accuracy of the adjustment are
displayed by means of SETUP, VECTRA, ELLIPA.
IDENT is used for labelling the XGP output, if any;
INTERNAL PROCEDURE DISCAL (STRING IDENT; REAL SW; INTEGER N; REFERENCE INTEGER P,Q;
REAL ARRAY X,Y,XP,YP,G,S; REFERENCE REAL SD);
BEGIN
REAL ARRAY A,B,C,CP[1:44],H[1:44,1:44],XD,YD,XR,YR[1:N],DT[1:2],DC[1:44,1:2],
DH[1:990,1:2];
REAL R,XX,YY,ACC,SD2;
INTEGER PNT,CHAN,NUM;
BOOLEAN FLAG;
OUTSTR(CRLF);
SETUP(SW,CHAN);
FOR PNT←1 STEP 1 UNTIL N DO
BEGIN
XD[PNT] ← XP[PNT]-X[PNT];
YD[PNT] ← YP[PNT]-Y[PNT]
END;
BEGIN
REAL XL,YL,XH,YH;
SCREEM(XL,YL,XH,YH);
SCREEN(XL,YL-.2*(YH-YL),
2*(XH-XL)+XL+.4*(XH-XL),2*(YH-YL)+YL+.2*(YH-YL));
END;
VECTRA(IDENT,"DISCREPANCIES",CHAN,N,X,Y,XD,YD);
IF P≥0 ∧ P≤5 ∧ Q≤10 THEN
BEGIN "MAIN LOOP"
PQINIT(P,Q);
FOR I←1 STEP 1 UNTIL M DO DC[I,1]←DC[I,2]←0;
K ← M*(M+1)%2;
FOR I←1 STEP 1 UNTIL K DO DH[I,1]←DH[I,2]←0;
FOR PNT←1 STEP 1 UNTIL N DO
BEGIN
DMODEL(A,B,X[PNT],Y[PNT]);
FOR I←1 STEP 1 UNTIL MG, M3 STEP 1 UNTIL M DO
BEGIN
MKD(DT[1],A[I]*XD[PNT]);
DAD(DC[I,1],DT[1]);
K ← I*(I-1)%2;
FOR J←1 STEP 1 UNTIL MG MIN I, M3 STEP 1 UNTIL I DO
BEGIN
MKD(DT[1],A[I]*A[J]);
DAD(DH[K+J,1],DT[1])
END;
END;
FOR I←M1 STEP 1 UNTIL M DO
BEGIN
MKD(DT[1],B[I]*YD[PNT]);
DAD(DC[I,1],DT[1]);
K ← I*(I-1)%2;
FOR J←M1 STEP 1 UNTIL I DO
BEGIN
MKD(DT[1],B[I]*B[J]);
DAD(DH[K+J,1],DT[1])
END
END
END;
FOR I←1 STEP 1 UNTIL M DO C[I]←DC[I,1];
K←0;
FOR I←1 STEP 1 UNTIL M DO FOR J←1 STEP 1 UNTIL I DO
BEGIN
K←K+1;
H[I,J] ← H[J,I] ← DH[K,1];
END;
INVERT(M,S,H,FLAG);
IF FLAG THEN OUTSTR("SINGULAR H MATRIX IN DISCAL" & CRLF);
TRANS(M,G,S,C);
TRANS(M,CP,H,G);
ACC←R←0;
FOR I←1 STEP 1 UNTIL M DO
BEGIN
R ← R + C[I]↑2;
ACC ← ACC + (CP[I]-C[I])↑2
END;
ACC←SQRT(ACC/R);
OUTSTR("ACC. OF MATRIX INVERSION =" & CVG(ACC));
R←0;
FOR PNT←1 STEP 1 UNTIL N DO
BEGIN
DMODEL(A,B,X[PNT],Y[PNT]);
XX←0;
FOR I←1 STEP 1 UNTIL MG, M3 STEP 1 UNTIL M DO
XX ← XX + A[I]*G[I];
XR[PNT] ← XD[PNT]-XX;
YY←0;
FOR I←M1 STEP 1 UNTIL M DO
YY ← YY + B[I]*G[I];
YR[PNT] ← YD[PNT]-YY;
R ← R + XR[PNT]↑2 + YR[PNT]↑2;
END;
SD2 ← R/(2*N-M);
SD ← SQRT(SD2);
FOR I←1 STEP 1 UNTIL M DO FOR J←1 STEP 1 UNTIL M DO S[I,J]←SD2*S[I,J];
OUTSTR(" STD. DEV. OF OBS. =" & CVG(SD) & CRLF);
BEGIN
REAL XL,YL,XH,YH;
SCREEM(XL,YL,XH,YH);
SCREEN(XL-(XH-XL)/2,YL,XH-(XH-XL)/2,YH);
END;
VECTRA("P="&CVS(P)&" Q="&CVS(Q)&" "&IDENT,"RESIDUALS",CHAN,N,X,Y,XR,YR,30);
NUM←8;
BEGIN
REAL XL,YL,XH,YH;
SCREEM(XL,YL,XH,YH);
SCREEN(XL+(XH-XL)/2,YL-(YH-YL)/2+.07*(YH-YL),
XH+(XH-XL)/2,YH-(YH-YL)/2+.07*(YH-YL));
END;
IF NUM>0 THEN
BEGIN
REAL ARRAY XA,YA[1:NUM*((3*NUM+2) DIV 4)];
REAL INC,X0;
INC←2/NUM;
X0←-1+1/NUM;
PNT←0;
FOR YY ← -((3*NUM-2) DIV 4)/NUM STEP INC UNTIL .75 DO
FOR XX←X0 STEP INC UNTIL 1 DO
BEGIN
PNT←PNT+1;
XA[PNT] ← XX*SW;
YA[PNT] ← YY*SW
END;
ELLIPA("P="&CVS(P)&" Q="&CVS(Q)&" "&
IDENT,"ACCURACY",CHAN,P,Q,PNT,XA,YA,S);
END;
DPYUP(CHAN); DPYUP(CHAN); DPYUP(CHAN);
END "MAIN LOOP";
P←PP;
DPYUP(CHAN); DPYUP(CHAN); DPYUP(CHAN);
END "DISCAL";
COMMENT DISREM uses the distortion calibration coefficients G as computed by
DISCAL to convert the N points XI,YI into the N points XO,YO. (XO and YO
can refer to the same arrays as XI and YI.) If TOL<0, the forward
conversion is done (X,Y to XP,YP in DISCAL). If TOL≥0, the inverse
conversion is done (XP,YP to X,Y in DISCAL), and TOL is the convergence
tolerance (in XO,YO) for terminating the iterations (with a maximum
of 10 iterations). (PQINIT must be called before this procedure to
compute the functions of P and Q, unless this has already been
done by calling DISCAL);
INTERNAL SIMPLE PROCEDURE DISREM (INTEGER N; REAL TOL;
REAL ARRAY G,XI,YI,XO,YO);
BEGIN
OWN REAL ARRAY A,B[1:44];
SAFE OWN REAL ARRAY XY,D[1:2];
REAL XX,YY,TOL2;
INTEGER PNT,ITER;
TOL2←TOL↑2;
FOR PNT←1 STEP 1 UNTIL N DO
BEGIN "POINTS"
XY[1]←XI[PNT];
XY[2]←YI[PNT];
FOR ITER←1 STEP 1 UNTIL 10 DO
BEGIN
DMODEL(A,B,XY[1],XY[2]);
XX←0;
FOR I←1 STEP 1 UNTIL MG, M3 STEP 1 UNTIL M DO
XX ← XX + A[I]*G[I];
YY←0;
FOR I←M1 STEP 1 UNTIL M DO
YY ← YY + B[I]*G[I];
IF TOL<0 THEN
BEGIN
XO[PNT] ← XI[PNT]+XX;
YO[PNT] ← YI[PNT]+YY;
CONTINUE "POINTS";
END
ELSE
BEGIN
XX ← XI[PNT]-XX;
YY ← YI[PNT]-YY;
D[1] ← XX-XY[1];
D[2] ← YY-XY[2];
ACCELERATE(2,XY,D,ITER=1,K);
END;
IF (D[1]↑2+D[2]↑2)<TOL2 THEN DONE;
END;
XO[PNT]←XY[1];
YO[PNT]←XY[2];
END "POINTS";
END "DISREM";
END "DISTOR";